function poseStandardize( tracksdir, outdir )
%poseSmoothing - Smooths (and interpolated if needed) the pose tracks.

% Examples:
%   tracksdir = ['/Users/trananh/Desktop/gestures/KTH/ground-truth/' ...
%     'walking/person01_walking_d1_uncomp/00001-00076/skeleton-tracks/'];
%   outdir = ['/Users/trananh/Desktop/gestures/KTH/ground-truth/' ...
%     'walking/person01_walking_d1_uncomp/00001-00076/skeleton-tracks-std/'];
  
  % Set default smoothing method to rloess
  if ~strcmp(outdir(end), '/')
    outdir = [outdir '/'];
  end
  if ~strcmp(tracksdir(end), '/')
    tracksdir = [tracksdir '/'];
  end
  
  % Create dir for output if necessary
  if ~exist(outdir, 'dir')
    mkdir(outdir);
  end
  
  prefix = '';
  
  % Retrieve all tracks
  fprintf('Standardizing dir: %s\n', tracksdir);
  fileslist = dir([tracksdir '*.txt']);
  
  % For each track, load and standardize the pose
  for i = 1:numel(fileslist)
    file = fileslist(i).name;
    fprintf(' - file %s\n', file);
    
    % Import data
    track = importdata([tracksdir file]);
    
    % Smooth pose data
    std = standardizepose(track.data);
    
    % Write back smoothed data
    outfile = [outdir prefix file];
    fid = fopen(outfile, 'w');
    heading = track.colheaders;
    for j=1:numel(heading)
      if j < numel(heading)
        fprintf(fid, '%s,', heading{j});
      else
        fprintf(fid, '%s\n', heading{j});
      end
    end
    fclose(fid);
    dlmwrite(outfile, std, 'delimiter', ',', 'precision', '%.5f', ...
      '-append');
  end
  
end

function std = standardizepose(data)
%standardizepose - Standardizes the pose track by taking the pose at
% _each frame_ and scaling it to fit within a 1x1 square.
  
  % First column is frame
  std = zeros(size(data));
  std(:,1) = data(:,1);
  
  xidx = 2:2:size(data,2);
  yidx = 3:2:size(data,2);
  
  top = min(data(:,yidx), [], 2);
  bottom = max(data(:,yidx), [], 2);
  left = min(data(:,xidx), [], 2);
  right = max(data(:,xidx), [], 2);
  
  rise = bottom - top;
  run = right - left;
  
  for y = yidx
    std(:,y) = (data(:,y) - top) ./ rise;
  end
  
  for x = xidx
    std(:,x) = (data(:,x) - left) ./ run;
  end

end

function std = standardizepose2(data)
%standardizepose2 - Standardizes the pose track by centering the pose
% at each frame such that the center of the pose is at 0,0.
  
  % First column is frame
  std = zeros(size(data));
  std(:,1) = data(:,1);
  
  xidx = 2:2:size(data,2);
  yidx = 3:2:size(data,2);
  
  top = min(data(:,yidx), [], 2);
  bottom = max(data(:,yidx), [], 2);
  left = min(data(:,xidx), [], 2);
  right = max(data(:,xidx), [], 2);
  
  midvert = (bottom - top) / 2;
  midhor = (right - left) / 2;
  
  for y = yidx
    std(:,y) = (data(:,y) - midvert);
  end
  
  for x = xidx
    std(:,x) = (data(:,x) - midhor);
  end

end